/*******************************************************************
 *
 * Copyright (C), 2022-2027, YouGan, All rights reserved.
 *
 * Author      : zhaoyang
 * Create Time : 2022-10-13
 * Version     : v0.1
 * Description : 
 *******************************************************************/

/*----------------------------Head file----------------------------*/
#include "gs_reg_lis3dh.h"
#include "app_gs_lis3dh.h"
#include "nrf_drv_twi.h"
#include "nrf_delay.h"
#include "gpio_i2c.h"

#include "board_cfg.h"

#define APP_GS_LOG_EN   	1

#if APP_GS_LOG_EN
#include "nrf_log.h"
#define APP_GS_LOG				NRF_LOG_INFO
#else
#define APP_GS_LOG(...)
#endif


/*----------------------------macro file---------------------------*/

#define GS_ONCE_USED_BYTES   0x03
#define GS_ONCE_READ_BYTES   0x06
#define GS_FIFO_MAX_CNT      0x08
#define GS_FIFO_MAX_BYTES    (GS_FIFO_MAX_CNT * GS_ONCE_READ_BYTES)
#define GS_ERR_MAX_CNT       (0x24*3)


/* Gensor I2C PIN 和address */
#define I2C_GSENSOR_ADDRESS                	0x19	

//Gensor ID
#define GSENSOR_ID                          0x33

#define USE_GPIO_I2C

/*----------------------------type define--------------------------*/
typedef struct
{
    uint8_t run_mode;
    uint8_t error_count;
    uint8_t read_data[GS_FIFO_MAX_BYTES];
	uint16_t backup_data[3];
    uint16_t exchange_data[3];
}gs_work_state_t;

/*----------------------------var define---------------------------*/
bool gs_start = false;

static gs_work_state_t gs_work_state;

#ifdef USE_GPIO_I2C
sw_i2c_gpio_t gs_i2c_gpio_cfg =
{
    .SCL_Pin = I2C_GSENSOR_PIN_SLC,
    .SDA_Pin = I2C_GSENSOR_PIN_SDA,
};

#else
//定义TWI驱动程序实例:0:TWI0  1:TWI1
static const nrf_drv_twi_t Twi_Gensor = NRF_DRV_TWI_INSTANCE(1);
#endif

/*-------------------------func declarative------------------------*/

/*-----------------------------------------------------------------*/

bool I2c_write(uint8_t register_address, uint8_t* value, uint8_t len)
{
    ret_code_t err_code = NRF_SUCCESS;
    uint8_t tx_buf[10];

    if (len > 9)
    {
        len = 9;
    }

    //准备写入的数据
    tx_buf[0] = register_address;
    memcpy((tx_buf + 1), value, len);
    
    //写入数据
#ifdef USE_GPIO_I2C
    err_code = sw_i2c_write_nBytes(&gs_i2c_gpio_cfg, I2C_GSENSOR_ADDRESS, tx_buf, (len + 1));
#else
    err_code = nrf_drv_twi_tx(&Twi_Gensor, I2C_GSENSOR_ADDRESS, tx_buf, (len + 1), false);
#endif

    if (NRF_SUCCESS != err_code)
    {
        APP_GS_LOG("I2c_write tx error = %d", err_code);
        return false;
    }
    
    return true;	
}

#ifdef USE_GPIO_I2C

#else
static bool I2c_read(uint8_t register_address, uint8_t* destination, uint8_t number_of_bytes)
{
    ret_code_t err_code;
    
    err_code = nrf_drv_twi_tx(&Twi_Gensor, I2C_GSENSOR_ADDRESS, &register_address, 1, true);
    if (NRF_SUCCESS != err_code)
    {
        APP_GS_LOG("I2c_read tx error = %d", err_code);
        return false;
    }

    err_code = nrf_drv_twi_rx(&Twi_Gensor, I2C_GSENSOR_ADDRESS, destination, number_of_bytes);
    if (NRF_SUCCESS != err_code)
    {
        APP_GS_LOG("I2c_read rx error = %d", err_code);
        return false;
    }
    return true;
}
#endif

static bool app_gs_write(uint8_t reg, uint8_t data)
{
	return I2c_write(reg, &data, 1);	
}


static bool app_gs_write_ex(uint8_t reg, uint8_t *pData, uint32_t len)
{
	return I2c_write((0x80|reg), pData, len);
}


static uint8_t app_gs_read(uint8_t reg)
{
	uint8_t rx = 0;
	
#ifdef USE_GPIO_I2C
	sw_i2c_send2read_8bit(&gs_i2c_gpio_cfg, I2C_GSENSOR_ADDRESS, (reg), &rx, 1);
#else
    I2c_read((reg), &rx, 1);
#endif

	return rx;
}


static void app_gs_read_ex(uint8_t reg, uint8_t *pData, uint32_t len)
{
#ifdef USE_GPIO_I2C
    sw_i2c_send2read_8bit(&gs_i2c_gpio_cfg, I2C_GSENSOR_ADDRESS, (0x80|reg), pData, len);
#else
    I2c_read((0x80|reg), pData, len);
#endif
}


static void gsensor_runmode_set(uint8_t u8RunMode)
{
	gs_work_state.run_mode = u8RunMode;
	gs_work_state.error_count = 0;
}


/*****************************************************************************
 * 函数功能  : 设置gsensor运行模式
 * 输入参数  : mode  运行模式
               运行25Hz 没有中断的模式，需要定时读取
 * 输出参数  : 无
*****************************************************************************/
static void app_gs_runmode_switch(void)
{
    uint8_t  buf[6];

    // 根据不同采用模式，配置Gsensor
    buf[0] = 0x37;     // CTRL_REG1,25Hz,xyz enable
    buf[1] = 0x00;     // CTRL_REG2,high pass filter disable
    buf[2] = 0x00;     // CTRL_REG3,no INT
	buf[3] = 0x18;     // CTRL_REG4,4G full scale (3DH 0x18-12bit, 0x10-10bit, 0x10-8bit)
    buf[4] = 0x40;     // CTRL_REG5,enable FIFO
    buf[5] = 0x00;     // CTRL_REG6,INT2 disable

    app_gs_write_ex(GS_CTRL_REG1, buf, 6);	// 配置所有控制寄存器

    app_gs_write(GS_FIFO_CTRL_REG, 0x40);	// set fifo mode
    nrf_delay_ms(10);

	gsensor_runmode_set(GS_MODE_WM_25HZ);
}


static void gsensor_data_check(void)
{
	for (uint8_t i = 0; i < 3; i++)
	{
		if (gs_work_state.backup_data[i] == gs_work_state.exchange_data[i])
		{
			gs_work_state.error_count++;
		}
		else
		{
			gs_work_state.error_count = 0;
		}

		gs_work_state.backup_data[i] = gs_work_state.exchange_data[i];

		if (gs_work_state.error_count >= GS_ERR_MAX_CNT)
		{
			gs_work_state.error_count = 0;
			gsensor_runmode_set(GS_MODE_ERR);
			APP_GS_LOG("gs ERR1.\n");
            //重新初始化
            app_gsensor_init();
            
			break;
		}
	}
}

//*****************************************************************************
// * 函 数 名  : Gsensor_I2C_Master_Init
// * 函数功能  : 3轴驱动I2C初始化
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void app_gs_acc_collect(void)
{
	uint8_t FifoCnt = 0;
	uint8_t FifoBytes = 0;

	FifoCnt = app_gs_read(GS_SRC_REG);
	
//    APP_GS_LOG("FifoCnt = %d", FifoCnt);

    //缓存满，转换模式可以清空缓存，重新开始
    if (FifoCnt & 0x40)
    {
        app_gs_write(GS_FIFO_CTRL_REG, 0x00);   // set bypass mode
        nrf_delay_ms(10);
        app_gs_write(GS_FIFO_CTRL_REG, 0x40);   // set fifo mode

        return;
    }
    
	FifoCnt     = FifoCnt & 0x1F;
	FifoBytes   = FifoCnt * GS_ONCE_READ_BYTES;

	
    //数据为空，也做转换模式操作，期望恢复正常
	if (FifoBytes == 0)
	{
		APP_GS_LOG("gs ERR2.\n");
		
        app_gs_write(GS_FIFO_CTRL_REG, 0x00);   // set bypass mode
        nrf_delay_ms(10);
        app_gs_write(GS_FIFO_CTRL_REG, 0x40);   // set fifo mode
        nrf_delay_ms(10);

//		gsensor_runmode_set(GS_MODE_ERR);

		//重新初始化
//		app_gsensor_init();

		return;
	}
	else if (FifoBytes >= GS_FIFO_MAX_BYTES)
	{
		FifoCnt = GS_FIFO_MAX_CNT;
		FifoBytes = GS_FIFO_MAX_BYTES;
	}

   	app_gs_read_ex(GS_OUT_X_L, gs_work_state.read_data, FifoBytes);

	for(uint8_t i = 0; i < FifoCnt; i++)
	{
		gs_work_state.exchange_data[0] = ((uint16_t) gs_work_state.read_data[i*6 + 1] << 8) + gs_work_state.read_data[i*6 + 0];
		gs_work_state.exchange_data[1] = ((uint16_t) gs_work_state.read_data[i*6 + 3] << 8) + gs_work_state.read_data[i*6 + 2];
		gs_work_state.exchange_data[2] = ((uint16_t) gs_work_state.read_data[i*6 + 5] << 8) + gs_work_state.read_data[i*6 + 4];
//		APP_GS_LOG("gs: %d, %d, %d.\n", (int16_t)gs_work_state.exchange_data[0], (int16_t)gs_work_state.exchange_data[1], (int16_t)gs_work_state.exchange_data[2]);

		if (true == app_hr_mod_init_state_get())
		{
			app_hr_gsensor_data_queue_put(gs_work_state.exchange_data);
		}
		gsensor_data_check();
	}

}

#ifdef USE_GPIO_I2C

#else

//*****************************************************************************
// * 函 数 名  : Gsensor_I2C_Master_Init
// * 函数功能  : 3轴驱动I2C初始化
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
ret_code_t Gsensor_I2C_Master_Init(void)
{
    ret_code_t ret;
    const nrf_drv_twi_config_t config =
    {
       .scl                = I2C_GSENSOR_PIN_SLC,
       .sda                = I2C_GSENSOR_PIN_SDA,
       .frequency          = NRF_DRV_TWI_FREQ_400K,
       .interrupt_priority = APP_IRQ_PRIORITY_HIGH,
       .clear_bus_init     = false
    };

    ret = nrf_drv_twi_init(&Twi_Gensor, &config, NULL, NULL);

    if (NRF_SUCCESS == ret)
    {
        nrf_drv_twi_enable(&Twi_Gensor);
    }

    return ret;
}

//*****************************************************************************
// * 函 数 名  : Gsensor_I2C_Master_DeInit
// * 函数功能  : 3轴驱动I2C卸载
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
static void Gsensor_I2C_Master_DeInit(void)
{
	//卸载I2C
    nrf_drv_twi_disable(&Twi_Gensor);

    nrf_drv_twi_uninit(&Twi_Gensor);
}
#endif

//*****************************************************************************
// * 函 数 名  : app_gsensor_init
// * 函数功能  : 3轴gsensor初始化
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void app_gsensor_init(void)
{
	app_gsensor_deinit();
	nrf_delay_ms(30);

#ifdef  USE_GPIO_I2C
    sw_i2c_init(&gs_i2c_gpio_cfg);
#else
    Gsensor_I2C_Master_Init();
    
#endif
	nrf_delay_ms(10);

    //配置三轴工作模式
	app_gs_runmode_switch();
	
	uint8_t id = app_gs_read(GS_WHO_AM_I);
	APP_GS_LOG("gs id: %x.\n", id);
	if (GSENSOR_ID != id)
	{
		APP_GS_LOG("gs get err id");
	}
	else
	{
    	gs_start = true;
	}
}

//*****************************************************************************
// * 函 数 名  : app_gsensor_deinit
// * 函数功能  : 3轴gsensor卸载
// * 输入参数  :	 无
// * 输出参数  : 无
// * 返 回 值  : 无
//*****************************************************************************/
void app_gsensor_deinit(void)
{
    gs_start = false;

#ifdef  USE_GPIO_I2C
#else
    Gsensor_I2C_Master_DeInit();
        
#endif
}

